#!/usr/bin/env python
# Log GPS data in the CSV format required by the rules

import csv
from datetime import datetime
import os.path
import rospy
from sensor_msgs.msg import NavSatFix
from sailing_robot.msg import gpswtime

RECORDS_DIR = os.path.expanduser('~/sailing-robot')
log_name = rospy.get_param('/log_name')

def record():
    filename = 'gps-trace_{}_{}.csv'.format(log_name,
                 datetime.now().strftime("%Y-%m-%dT%H.%M.%S"))
    day_of_month = datetime.now().day
    with open(os.path.join(RECORDS_DIR, filename), 'w', 0) as f:
        csvw = csv.writer(f)
        def write(msg):
            ts = '%02d%02d%02d%02d' % (msg.time_h, msg.time_m, msg.time_s,
                                        day_of_month)
            lat = int(msg.fix.latitude * 1e7)
            lon = int(msg.fix.longitude * 1e7)
            csvw.writerow([ts, lat, lon])

        rospy.Subscriber('gps_fix', gpswtime, write)
        rospy.spin()

if __name__ == '__main__':
    try:
        rospy.init_node("debugging_gps_log", anonymous=True)
        record()
    except rospy.ROSInterruptException:
        pass
